#include "vehicle_lateral_mpc.h"

namespace auto_ros
{
namespace control
{

VehicleLateralMpc::VehicleLateralMpc(const std::string dyn_config_filename, const std::string mpc_config_filename, double rate)
	: rate_(rate)
{

	YAML::Node dyn_yaml = YAML::LoadFile(dyn_config_filename);
	mpc_yaml_ = YAML::LoadFile(mpc_config_filename);
	//VehicleLinearLv(double mass, double cf, double cr, double lf, double lr,
	//double ix, double iz, double h, double k_phi, double d_phi, double max_lateral_acc = 9,
	//double max_tire_f_angle = 0.5);
	vehicle_dyn_ptr_ = std::make_shared<VehicleLinearLateral>(
		dyn_yaml["mass"].as<double>(), dyn_yaml["cf"].as<double>(), dyn_yaml["cr"].as<double>(),
		dyn_yaml["lf"].as<double>(), dyn_yaml["lr"].as<double>(), dyn_yaml["iz"].as<double>(),
		dyn_yaml["max_lateral_acc"].as<double>(), dyn_yaml["max_tire_f_angle"].as<double>());
	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_, "tustin");
	/*	Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr,
		unsigned int mpc_windows,
		unsigned int max_xu_constraint,//1
		unsigned int max_delta_x,//0
		unsigned int max_delta_u,//1
		unsigned int max_final_constraint,//0
		unsigned int max_xu_ae =0,//0
		unsigned int max_xu_slack_ineq = 0, //2
		unsigned int max_x_final_ae = 0,
		unsigned int max_x_final_slack_ineq = 0, //0
		unsigned int max_delta_u_ae = 1,
		unsigned int max_delta_u_slack_ineq = 0, // 0
		unsigned int max_delta_x_ae = 0,
		unsigned int max_delta_x_slack_ineq = 0);*/
	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 1 /*max_xu_constraint*/, 0, 1 /*max_delta_u*/, 0, 0,
											 2 /*max_xu_slack_ineq = 0*/, 0, 0, 1 /*max_delta_u_ae*/, 0, 0, 0);
	/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)*/
	//init constriants or add add static constriants
	/*int Mpc::set_xu_slack_ineq(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)*/
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1.0}, 0, -dyn_yaml["max_tire_f_angle"].as<double>() / 180 * 3.14,
									dyn_yaml["max_tire_f_angle"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1, vehicle_dyn_ptr_->lf() / 1.0}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1, -vehicle_dyn_ptr_->lr() / 1.0}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);

	vehicle_mpc_ptr_->set_delta_u_ae(0, 0, 0);
	vehicle_mpc_ptr_->set_delta_u_ae_w({mpc_yaml_["delta_tire_steering_f_w"].as<double>() * rate * rate});

	vehicle_mpc_ptr_->set_xu_slack_ineq_w({100, 100});
	x_ref_.resize(dis_dyn_ptr_->state_dim());
	u_ref_.resize(dis_dyn_ptr_->control_dim());
	//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_rate"].as<double>() / rate_);
} //

bool VehicleLateralMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
							 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
							 double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
							 double tire_r_alpha_w)
{

	vehicle_dyn_ptr_->speed_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool VehicleLateralMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
							 const Eigen::MatrixXd &distur_ref, double speed)
{
	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["lateral_speed_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["yaw_rate_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["heading_error_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	if (std::abs(x0[2]) <= 0.01 && std::abs(x0[3]) < 0.01) //std::abs(distur_ref(0, 0)) < 0.001
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_max"].as<double>();
	}
	else
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_min"].as<double>();
	}

	Eigen::VectorXd x_ref(dis_dyn_ptr_->state_dim());
	Eigen::VectorXd u_ref(dis_dyn_ptr_->control_dim());
	Eigen::VectorXd e_y_ref(1);
	e_y_ref << 0.0;
	if (std::abs(speed) < vehicle_dyn_ptr_->speed_threshold())
	{
		if (speed >= 0)
			speed = vehicle_dyn_ptr_->speed_threshold();
		else
			speed = -vehicle_dyn_ptr_->speed_threshold();
	}
	vehicle_dyn_ptr_->speed_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)*/
	vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);

	x_ref_ = x_ref;
	u_ref_ = u_ref;
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1, vehicle_dyn_ptr_->lf() / speed}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1, -vehicle_dyn_ptr_->lr() / speed}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);

	double tire_f_alpha_w = mpc_yaml_["tire_f_alpha_w"].as<double>();
	double tire_r_alpha_w = mpc_yaml_["tire_r_alpha_w"].as<double>();

	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q_temp, r_temp, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
YAML::Node VehicleLateralMpc::mpc_yaml()
{
	return mpc_yaml_;
}

} // namespace control
} // namespace auto_ros
